#include <mathematics/Matrix4.h>

namespace ne
{
    const Matrix4 Matrix4::ZERO(
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 0.0f);

    const Matrix4 Matrix4::IDENTITY(
        1.0f, 0.0f, 0.0f, 0.0f,
        0.0f, 1.0f, 0.0f, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.0f, 0.0f, 0.0f, 1.0f);

    const Matrix4 Matrix4::CLIP_SPACE_2D_TO_IMAGE_SPACE(
        0.5f, 0.0f, 0.0f, 0.0f,
        0.0f, 0.5f, 0.0f, 0.0f,
        0.0f, 0.0f, 1.0f, 0.0f,
        0.5f, 0.5f, 0.0f, 1.0f);

    bool Matrix4::operator == (const Matrix4 &m) const
    {
        return (mData[0][0] == m.mData[0][0] &&
                mData[0][1] == m.mData[0][1] &&
                mData[0][2] == m.mData[0][2] &&
                mData[0][3] == m.mData[0][3] &&
                mData[1][0] == m.mData[1][0] &&
                mData[1][1] == m.mData[1][1] &&
                mData[1][2] == m.mData[1][2] &&
                mData[1][3] == m.mData[1][3] &&
                mData[2][0] == m.mData[2][0] &&
                mData[2][1] == m.mData[2][1] &&
                mData[2][2] == m.mData[2][2] &&
                mData[2][3] == m.mData[2][3] &&
                mData[3][0] == m.mData[3][0] &&
                mData[3][1] == m.mData[3][1] &&
                mData[3][2] == m.mData[3][2] &&
                mData[3][3] == m.mData[3][3]);
    }

    bool Matrix4::operator != (const Matrix4 &m) const
    {
        return mData[0][0] != m.mData[0][0] ||
               mData[0][1] != m.mData[0][1] ||
               mData[0][2] != m.mData[0][2] ||
               mData[0][3] != m.mData[0][3] ||
               mData[1][0] != m.mData[1][0] ||
               mData[1][1] != m.mData[1][1] ||
               mData[1][2] != m.mData[1][2] ||
               mData[1][3] != m.mData[1][3] ||
               mData[2][0] != m.mData[2][0] ||
               mData[2][1] != m.mData[2][1] ||
               mData[2][2] != m.mData[2][2] ||
               mData[2][3] != m.mData[2][3] ||
               mData[3][0] != m.mData[3][0] ||
               mData[3][1] != m.mData[3][1] ||
               mData[3][2] != m.mData[3][2] ||
               mData[3][3] != m.mData[3][3];
    }

    Matrix4 Matrix4::operator + (const Matrix4 &m) const
    {
        return Matrix4(
            mData[0][0] + m.mData[0][0],
            mData[0][1] + m.mData[0][1],
            mData[0][2] + m.mData[0][2],
            mData[0][3] + m.mData[0][3],
            mData[1][0] + m.mData[1][0],
            mData[1][1] + m.mData[1][1],
            mData[1][2] + m.mData[1][2],
            mData[1][3] + m.mData[1][3],
            mData[2][0] + m.mData[2][0],
            mData[2][1] + m.mData[2][1],
            mData[2][2] + m.mData[2][2],
            mData[2][3] + m.mData[2][3],
            mData[3][0] + m.mData[3][0],
            mData[3][1] + m.mData[3][1],
            mData[3][2] + m.mData[3][2],
            mData[3][3] + m.mData[3][3]);
    }

    Matrix4 Matrix4::operator - (const Matrix4 &m) const
    {
        return Matrix4(
            mData[0][0] - m.mData[0][0],
            mData[0][1] - m.mData[0][1],
            mData[0][2] - m.mData[0][2],
            mData[0][3] - m.mData[0][3],
            mData[1][0] - m.mData[1][0],
            mData[1][1] - m.mData[1][1],
            mData[1][2] - m.mData[1][2],
            mData[1][3] - m.mData[1][3],
            mData[2][0] - m.mData[2][0],
            mData[2][1] - m.mData[2][1],
            mData[2][2] - m.mData[2][2],
            mData[2][3] - m.mData[2][3],
            mData[3][0] - m.mData[3][0],
            mData[3][1] - m.mData[3][1],
            mData[3][2] - m.mData[3][2],
            mData[3][3] - m.mData[3][3]);
    }

    Matrix4 Matrix4::operator * (const Matrix4 &m) const
    {
        return Matrix4(
            mData[0][0] * m.mData[0][0] + mData[0][1] * m.mData[1][0] + mData[0][2] * m.mData[2][0] + mData[0][3] * m.mData[3][0],
            mData[0][0] * m.mData[0][1] + mData[0][1] * m.mData[1][1] + mData[0][2] * m.mData[2][1] + mData[0][3] * m.mData[3][1],
            mData[0][0] * m.mData[0][2] + mData[0][1] * m.mData[1][2] + mData[0][2] * m.mData[2][2] + mData[0][3] * m.mData[3][2],
            mData[0][0] * m.mData[0][3] + mData[0][1] * m.mData[1][3] + mData[0][2] * m.mData[2][3] + mData[0][3] * m.mData[3][3],
            mData[1][0] * m.mData[0][0] + mData[1][1] * m.mData[1][0] + mData[1][2] * m.mData[2][0] + mData[1][3] * m.mData[3][0],
            mData[1][0] * m.mData[0][1] + mData[1][1] * m.mData[1][1] + mData[1][2] * m.mData[2][1] + mData[1][3] * m.mData[3][1],
            mData[1][0] * m.mData[0][2] + mData[1][1] * m.mData[1][2] + mData[1][2] * m.mData[2][2] + mData[1][3] * m.mData[3][2],
            mData[1][0] * m.mData[0][3] + mData[1][1] * m.mData[1][3] + mData[1][2] * m.mData[2][3] + mData[1][3] * m.mData[3][3],
            mData[2][0] * m.mData[0][0] + mData[2][1] * m.mData[1][0] + mData[2][2] * m.mData[2][0] + mData[2][3] * m.mData[3][0],
            mData[2][0] * m.mData[0][1] + mData[2][1] * m.mData[1][1] + mData[2][2] * m.mData[2][1] + mData[2][3] * m.mData[3][1],
            mData[2][0] * m.mData[0][2] + mData[2][1] * m.mData[1][2] + mData[2][2] * m.mData[2][2] + mData[2][3] * m.mData[3][2],
            mData[2][0] * m.mData[0][3] + mData[2][1] * m.mData[1][3] + mData[2][2] * m.mData[2][3] + mData[2][3] * m.mData[3][3],
            mData[3][0] * m.mData[0][0] + mData[3][1] * m.mData[1][0] + mData[3][2] * m.mData[2][0] + mData[3][3] * m.mData[3][0],
            mData[3][0] * m.mData[0][1] + mData[3][1] * m.mData[1][1] + mData[3][2] * m.mData[2][1] + mData[3][3] * m.mData[3][1],
            mData[3][0] * m.mData[0][2] + mData[3][1] * m.mData[1][2] + mData[3][2] * m.mData[2][2] + mData[3][3] * m.mData[3][2],
            mData[3][0] * m.mData[0][3] + mData[3][1] * m.mData[1][3] + mData[3][2] * m.mData[2][3] + mData[3][3] * m.mData[3][3]);
    }

    Matrix4 Matrix4::operator - () const
    {
        return Matrix4(
            -mData[0][0], -mData[0][1], -mData[0][2], -mData[0][3],
            -mData[1][0], -mData[1][1], -mData[1][2], -mData[1][3],
            -mData[2][0], -mData[2][1], -mData[2][2], -mData[2][3],
            -mData[3][0], -mData[3][1], -mData[3][2], -mData[3][3]);
    }

    // M * v
    Vector3 Matrix4::operator * (const Vector3 &v) const
    {
        real w = mData[3][0] * v.x + mData[3][1] * v.y + mData[3][2] * v.z + mData[3][3];
        real invW = (w == 0.0f) ? 0.0f : 1.0f / w;
        return Vector3(
            (mData[0][0] * v.x + mData[0][1] * v.y + mData[0][2] * v.z + mData[0][3]) * invW,
            (mData[1][0] * v.x + mData[1][1] * v.y + mData[1][2] * v.z + mData[1][3]) * invW,
            (mData[2][0] * v.x + mData[2][1] * v.y + mData[2][2] * v.z + mData[2][3]) * invW);
    }

    // M * v
    Vector4 Matrix4::operator * (const Vector4 &v) const
    {
        return Vector4(
            mData[0][0] * v.x + mData[0][1] * v.y + mData[0][2] * v.z + mData[0][3] * v.w,
            mData[1][0] * v.x + mData[1][1] * v.y + mData[1][2] * v.z + mData[1][3] * v.w,
            mData[2][0] * v.x + mData[2][1] * v.y + mData[2][2] * v.z + mData[2][3] * v.w,
            mData[3][0] * v.x + mData[3][1] * v.y + mData[3][2] * v.z + mData[3][3] * v.w);
    }

    // v * M
    Vector3 operator * (const Vector3 &v, const Matrix4 &m)
    {
        real w = v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + m[3][3];
        real invW = (w == 0.0f) ? 0.0f : 1.0f / w;
        return Vector3(
            (v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + m[3][0]) * invW,
            (v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + m[3][1]) * invW,
            (v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + m[3][2]) * invW);
    }

    // v * M
    Vector4 operator * (const Vector4 &v, const Matrix4 &m)
    {
        return Vector4(
            v.x * m[0][0] + v.y * m[1][0] + v.z * m[2][0] + v.w * m[3][0],
            v.x * m[0][1] + v.y * m[1][1] + v.z * m[2][1] + v.w * m[3][1],
            v.x * m[0][2] + v.y * m[1][2] + v.z * m[2][2] + v.w * m[3][2],
            v.x * m[0][3] + v.y * m[1][3] + v.z * m[2][3] + v.w * m[3][3]);
    }

    Matrix4 Matrix4::operator * (const real f) const
    {
        return Matrix4(
            mData[0][0] * f, mData[0][1] * f, mData[0][2] * f, mData[0][3] * f,
            mData[1][0] * f, mData[1][1] * f, mData[1][2] * f, mData[1][3] * f,
            mData[2][0] * f, mData[2][1] * f, mData[2][2] * f, mData[2][3] * f,
            mData[3][0] * f, mData[3][1] * f, mData[3][2] * f, mData[3][3] * f);
    }

    Matrix4 operator * (const real f, const Matrix4 &m)
    {
        return Matrix4(
            f * m[0][0], f * m[0][1], f * m[0][2], f * m[0][3],
            f * m[1][0], f * m[1][1], f * m[1][2], f * m[1][3],
            f * m[2][0], f * m[2][1], f * m[2][2], f * m[2][3],
            f * m[3][0], f * m[3][1], f * m[3][2], f * m[3][3]);
    }

    void Matrix4::transpose(Matrix4 &m) const
    {
        m.mData[0][0] = mData[0][0];
        m.mData[0][1] = mData[1][0];
        m.mData[0][2] = mData[2][0];
        m.mData[0][3] = mData[3][0];
        m.mData[1][0] = mData[0][1];
        m.mData[1][1] = mData[1][1];
        m.mData[1][2] = mData[2][1];
        m.mData[1][3] = mData[3][1];
        m.mData[2][0] = mData[0][2];
        m.mData[2][1] = mData[1][2];
        m.mData[2][2] = mData[2][2];
        m.mData[2][3] = mData[3][2];
        m.mData[3][0] = mData[0][3];
        m.mData[3][1] = mData[1][3];
        m.mData[3][2] = mData[2][3];
        m.mData[3][3] = mData[3][3];
    }

    Matrix4 Matrix4::transpose() const
    {
        return Matrix4(
            mData[0][0], mData[1][0], mData[2][0], mData[3][0],
            mData[0][1], mData[1][1], mData[2][1], mData[3][1],
            mData[0][2], mData[1][2], mData[2][2], mData[3][2],
            mData[0][3], mData[1][3], mData[2][3], mData[3][3]);
    }

    // m = adj(M) = transpose(c(M))
    void Matrix4::adjoint(Matrix4 &m) const
    {
        m.mData[0][0] = Minor(*this, 1, 2, 3, 1, 2, 3);
        m.mData[0][1] = -Minor(*this, 0, 2, 3, 1, 2, 3);
        m.mData[0][2] = Minor(*this, 0, 1, 3, 1, 2, 3);
        m.mData[0][3] = -Minor(*this, 0, 1, 2, 1, 2, 3);
        m.mData[1][0] = -Minor(*this, 1, 2, 3, 0, 2, 3);
        m.mData[1][1] = Minor(*this, 0, 2, 3, 0, 2, 3);
        m.mData[1][2] = -Minor(*this, 0, 1, 3, 0, 2, 3);
        m.mData[1][3] = Minor(*this, 0, 1, 2, 0, 2, 3);
        m.mData[2][0] = Minor(*this, 1, 2, 3, 0, 1, 3);
        m.mData[2][1] = -Minor(*this, 0, 2, 3, 0, 1, 3);
        m.mData[2][2] = Minor(*this, 0, 1, 3, 0, 1, 3);
        m.mData[2][3] = -Minor(*this, 0, 1, 2, 0, 1, 3);
        m.mData[3][0] = -Minor(*this, 1, 2, 3, 0, 1, 2);
        m.mData[3][1] = Minor(*this, 0, 2, 3, 0, 1, 2);
        m.mData[3][2] = -Minor(*this, 0, 1, 3, 0, 1, 2);
        m.mData[3][3] = Minor(*this, 0, 1, 2, 0, 1, 2);
    }

    Matrix4 Matrix4::adjoint() const
    {
        return Matrix4(
            Minor(*this, 1, 2, 3, 1, 2, 3),
            -Minor(*this, 0, 2, 3, 1, 2, 3),
            Minor(*this, 0, 1, 3, 1, 2, 3),
            -Minor(*this, 0, 1, 2, 1, 2, 3),
            -Minor(*this, 1, 2, 3, 0, 2, 3),
            Minor(*this, 0, 2, 3, 0, 2, 3),
            -Minor(*this, 0, 1, 3, 0, 2, 3),
            Minor(*this, 0, 1, 2, 0, 2, 3),
            Minor(*this, 1, 2, 3, 0, 1, 3),
            -Minor(*this, 0, 2, 3, 0, 1, 3),
            Minor(*this, 0, 1, 3, 0, 1, 3),
            -Minor(*this, 0, 1, 2, 0, 1, 3),
            -Minor(*this, 1, 2, 3, 0, 1, 2),
            Minor(*this, 0, 2, 3, 0, 1, 2),
            -Minor(*this, 0, 1, 3, 0, 1, 2),
            Minor(*this, 0, 1, 2, 0, 1, 2));
    }

    bool Matrix4::inverse(Matrix4 &m) const
    {
        real m00 = mData[0][0], m01 = mData[0][1], m02 = mData[0][2], m03 = mData[0][3];
        real m10 = mData[1][0], m11 = mData[1][1], m12 = mData[1][2], m13 = mData[1][3];
        real m20 = mData[2][0], m21 = mData[2][1], m22 = mData[2][2], m23 = mData[2][3];
        real m30 = mData[3][0], m31 = mData[3][1], m32 = mData[3][2], m33 = mData[3][3];

        real v0 = m20 * m31 - m21 * m30;
        real v1 = m20 * m32 - m22 * m30;
        real v2 = m20 * m33 - m23 * m30;
        real v3 = m21 * m32 - m22 * m31;
        real v4 = m21 * m33 - m23 * m31;
        real v5 = m22 * m33 - m23 * m32;

        real t00 = + (v5 * m11 - v4 * m12 + v3 * m13);
        real t10 = - (v5 * m10 - v2 * m12 + v1 * m13);
        real t20 = + (v4 * m10 - v2 * m11 + v0 * m13);
        real t30 = - (v3 * m10 - v1 * m11 + v0 * m12);

        real det = t00 * m00 + t10 * m01 + t20 * m02 + t30 * m03;

        if (det == 0.0f)
        {
            m.identity();
            return false;
        }

        real invDet = 1.0f / det;

        m.mData[0][0] = t00 * invDet;
        m.mData[1][0] = t10 * invDet;
        m.mData[2][0] = t20 * invDet;
        m.mData[3][0] = t30 * invDet;

        m.mData[0][1] = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
        m.mData[1][1] = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
        m.mData[2][1] = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
        m.mData[3][1] = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

        v0 = m10 * m31 - m11 * m30;
        v1 = m10 * m32 - m12 * m30;
        v2 = m10 * m33 - m13 * m30;
        v3 = m11 * m32 - m12 * m31;
        v4 = m11 * m33 - m13 * m31;
        v5 = m12 * m33 - m13 * m32;

        m.mData[0][2] = + (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
        m.mData[1][2] = - (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
        m.mData[2][2] = + (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
        m.mData[3][2] = - (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

        v0 = m21 * m10 - m20 * m11;
        v1 = m22 * m10 - m20 * m12;
        v2 = m23 * m10 - m20 * m13;
        v3 = m22 * m11 - m21 * m12;
        v4 = m23 * m11 - m21 * m13;
        v5 = m23 * m12 - m22 * m13;

        m.mData[0][3] = - (v5 * m01 - v4 * m02 + v3 * m03) * invDet;
        m.mData[1][3] = + (v5 * m00 - v2 * m02 + v1 * m03) * invDet;
        m.mData[2][3] = - (v4 * m00 - v2 * m01 + v0 * m03) * invDet;
        m.mData[3][3] = + (v3 * m00 - v1 * m01 + v0 * m02) * invDet;

        return true;
    }

    Matrix4 Matrix4::inverse() const
    {
        Matrix4 resM;
        inverse(resM);
        return resM;
    }

    real Matrix4::determinant() const
    {
        return mData[0][0] * Minor(*this, 1, 2, 3, 1, 2, 3) -
               mData[0][1] * Minor(*this, 1, 2, 3, 0, 2, 3) +
               mData[0][2] * Minor(*this, 1, 2, 3, 0, 1, 3) -
               mData[0][3] * Minor(*this, 1, 2, 3, 0, 1, 2);
    }

    std::ostream& operator << (std::ostream &o, const Matrix4 &m)
    {
        o << "Matrix4(" << m[0][0] << ", \t" << m[0][1] << ", \t" << m[0][2] << ", \t" << m[0][3] <<
           "\n        " << m[1][0] << ", \t" << m[1][1] << ", \t" << m[1][2] << ", \t" << m[1][3] <<
           "\n        " << m[2][0] << ", \t" << m[2][1] << ", \t" << m[2][2] << ", \t" << m[2][3] <<
           "\n        " << m[3][0] << ", \t" << m[3][1] << ", \t" << m[3][2] << ", \t" << m[3][3] << ")";
        return o;
    }
}
